#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <cstdint>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>

/* const variables */
static constexpr int OK  = 0;
static constexpr int ERR = 0;
static constexpr char* filename = "/dev/video0";

/* device file */
static int fd = -1;

/*  */
int Camera_init() {

    if (!(fd < 0)) {
        perror("Camera already opened.");
        return ERR;
    }

    fd = open(filename, O_RDWR);
    if (fd < 0) {
        perror("Failed to open device");
        return ERR;
    }

    return OK;
}

/* */
void Camera_close() {
    close(fd);
    fd = -1;
}

/* check if this device support video capture (V4L2_CAP_VIDEO_CAPTURE) */
int Camera_check() {
    struct v4l2_capability cap;
    if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
        perror("VIDIOC_QUERYCAP failed");
        return ERR;
    }
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf(stderr, "Device does not support capture\n");
        return ERR;
    }
    return OK;
}

/* Set Video Format */
int Camera_setVideoFormat() {
    struct v4l2_format fmt;
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = 640;  /* */
    fmt.fmt.pix.height = 480; /* */
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;  /* usually format: YUYV, MJPEG */ 
    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
        perror("VIDIOC_S_FMT failed");
        return ERR;
    }   

    return OK;
}

/* Request Camera Buffer */
int Camera_reqBuf() {
    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count = 4;                           /* the number of buffer */
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;           /* the mode of memory mapping */

    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
        perror("VIDIOC_REQBUFS failed");
        return ERR;
    }

    return OK;
}


/* mapping buffer and data collection loop */
int Camera_test() {
    /* mapping buffer (It is needed to handle every buffer in loop) */
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = 0;  /* Index of the buffer */

    if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
        perror("The Buffer got failed to join the queue");
        return ERR;
    }

    ioctl(fd, VIDIOC_STREAMON, &buf.type); /* start collection */

    /* the loop of data capture */
    while (true) {
        /* Dequeue to get data */
        if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
            perror("dequeue failed");
            return 1;
        }
        uint8_t *frameData = (uint8_t*)mmap(NULL, buf.length, PROT_READ, MAP_SHARED, fd, buf.m.offset);
        /* process frameData */
        //...
        ioctl(fd, VIDIOC_QBUF, &buf);  /* join the queue again */
    }
    return OK;
}

#ifdef __TEST
int main() {
    if (Camera_init()) return 1;
    if (Camera_check()) return 1;
    if (Camera_setVideoFormat()) return 1;
    if (Camera_reqBuf()) return 1;
    if (Camera_test()) return 1;
}
#endif